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SUMMARY 

This  report  describes  a  method  to  calculate  weapon  platform  attitude  relative  to  Earth  as  well 
as  determine  the  maximum  resulting  platform  cant  and  heading  of  that  cant  relative  to  the  weapon 
platform.  These  calculations  are  performed  using  sensors  that  may  already  exist  on  an  indirect 
weapon  system.  Calculation  of  platform  cant  is  an  important  consideration  in  operation  of  large 
caliber  mobile  weapons. 


INTRODUCTION 

U.S.  Army  weapon  systems  are  designed  to  be  deployed  and  operated  wherever  a  need 
arises.  This  design  requirement  necessitates  operation  on  a  myriad  of  terrain  types.  This  includes 
hills,  mountains  and  ravines-locations  where  level  ground  is  rare  or  unavailable.  It  follows  that 
successful  development  of  mobile  weapon  systems  must  incorporate  operation  on  sloped  terrain. 

Sloped  terrain  presents  challenges  for  firing  large  caliber  weapons.  When  a  weapon  is  fired, 
the  forward  momentum  of  the  discharge  is  equally  reflected  to  the  weapon  in  the  form  of  a  recoiling 
impulse.  That  recoiling  force  can  be  minimized  via  a  recoil  system,  which  applies  a  lesser  counter 
force  over  a  calculated  distance  to  spread  the  impulse  over  a  greater  period  of  time.  However,  even 
with  sophisticated  recoil  systems,  large  caliber  guns  can  impart  significant  forces  into  the  weapon 
mount  and  subsequently  the  weapon  platform.  Figure  1  illustrates  a  large  platform  cant  combined 
with  a  low  firing  angle  tangential  to  that  cant.  Firing  in  this  configuration  could  destabilize  the 
platform,  possibly  resulting  in  a  vehicle  rollover. 


Figure  1 
Excessive  cant 


Sloped  terrain  can  also  impact  non-firing  operations.  Traversing  on  a  level  platform  requires 
force  to  accelerate  the  inertia  of  the  mass  and  overcome  any  frictional  losses.  When  canted,  a 
gravitational  component  is  added.  That  additional  load  increases  with  the  cant  angle  and  is  reflected 
to  the  traversing  mechanism  requiring  additional  force  to  overcome.  There  are  also  instances  when 
cant  must  be  minimized  in  order  to  perform  certain  maintenance  procedures  such  as  boresighting 
the  gun  tube. 

Due  to  the  challenges  presented  by  operation  on  uneven  terrain,  determination  of  the 
weapon  cant  is  essential.  Computer  controlled  indirect  weapon  systems  typically  have  an  attitude 
sensing  device  that  is  aligned  to  the  gun  tube  and  enables  precise  pointing  of  the  weapon.  This 
device  provides  the  attitude  of  the  gun  tube  relative  to  Earth  using  a  series  of  rotations  (yaw,  pitch, 
and  roll)  called  Euler  angles.  In  addition,  these  systems  often  incorporate  sensors  to  indicate  the 
angles  of  the  traversing  and  elevating  actuators  relative  to  the  platform.  The  remainder  of  this  report 
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will  describe  how  to  use  the  weapon  attitude  and  actuator  sensor  data  to  calculate  the  attitude  of  the 
platform  relative  to  Earth  as  well  as  the  maximum  cant  and  heading  values. 

Coordinate  Systems 

In  order  to  solve  kinematic  equations,  the  frames  of  reference  must  be  defined.  This  report 
defines  the  Earth  reference  frame  (fig.  2)  as  an  orthogonal  system  with  the  origin  of  the  frame  placed 
at  the  weapon’s  location  on  the  Earth’s  surface.  From  the  origin,  north  is  the  X  axis,  the  Z  axis  points 
downward  toward  the  center  of  the  earth  sphere,  and  the  Y  axis  completes  the  orthogonal  reference 
frame  using  the  right  hand  rule.  Note  that  north  could  be  true  north,  magnetic  north,  grid  north,  etc., 
as  long  as  a  single  definition  is  applied  throughout. 


Figure  1 

Earth  reference  frame 

Actuators  traverse  and  elevate  the  gun  barrel  relative  to  the  system  platform.  The  gun  to 
platform  relationship  is  defined  using  a  reference  frame  that  is  different  than  the  Earth  reference 
frame  described  previously.  The  origin  of  the  platform  reference  frame  typically  exists  at  or  near  the 
intersection  of  the  traverse  axis  and  the  elevation  axis.  For  the  platform  reference  frame,  the  Z  axis 
extends  down  along  the  traverse  axis  of  rotation.  The  X  axis  extends  toward  what  is  established  to 
be  the  0  azimuth  reference  of  the  platform  at  an  angle  normal  to  the  Z  axis  (typically  what  would  be 
considered  the  front  of  the  platform).  The  Y  axis  completes  the  orthogonal  reference  frame  using  the 
right  hand  rule.  An  example  platform  reference  frame  is  illustrated  in  figure  3. 
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Figure  2 

Platform  reference  frame 

When  performing  rotation  transformations  between  reference  frames,  an  intermediate 
reference  frame  is  used  to  perform  the  operations.  This  is  referred  to  as  the  local  reference  frame. 
The  local  reference  frame  is  initially  coincident  with  the  starting  reference  frame.  It  is  subsequently 
rotated  about  its  axes  in  the  necessary  series  of  motions  that  conclude  with  it  being  coincident  with 
the  desired  reference  frame. 

The  Problem 

Determination  of  the  maximum  platform  inclination  angle  relative  to  Earth  (cant)  is  essential 
to  ensure  effective  and  safe  operation  of  a  weapon  system.  To  accomplish  this,  the  pitch  and  roll 
angles  of  the  platform  must  be  determined.  The  pitch  and  roll  angles  can  then  be  used  to  calculate 
the  maximum  cant  and  heading  for  comparison  with  vehicle  stability  limits.  The  following  paragraphs 
provide  equations  and  methods  to  calculate  those  solutions. 


METHODS,  ASSUMPTIONS,  AND  PROCEDURES 
Conventions  and  Variable  Definitions 

Before  describing  the  equations  to  solve  the  aforementioned  problems,  it  is  important  to 
define  the  conventions  and  variables  to  be  used.  This  report  assumes  a  system  that  outputs  Earth 
referenced  weapon  attitude  in  ZYX  Euler  angles.  Other  conventions  would  require  recalculation  of 
the  equations. 

Table  1  defines  the  variables  used  in  the  provided  equations.  Subscripts  are  used  to  identify 
the  object  being  measured  and/or  the  reference  frame.  Angles  are  represented  using  radians. 
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Table  1 

Variable  definitions 


Variable 

Definition 

Xe,  Ye,  Ze 

Axes  of  Earth  reference  frame 

Xp,  Yp,  Zp 

Axes  of  platform  reference  frame 

x,  Y,  Z 

Axes  of  the  local  reference  frame 

IjJGe 

Gun  attitude  yaw  rotation  (Earth  reference) 

0Ge 

Gun  attitude  pitch  rotation  (Earth  reference) 

CP  Ge 

Gun  attitude  roll  rotation  (Earth  reference) 

MJGp 

Gun  attitude  yaw  rotation  (Platform  reference) 

0Gp 

Gun  attitude  pitch  rotation  (Platform  reference) 

IflPe 

Platform  attitude  yaw  rotation  (Earth  reference) 

0Pe 

Platform  attitude  pitch  rotation  (Earth  reference) 

(P  Pe 

Platform  attitude  roll  rotation  (Earth  reference) 

Cpe 

Platform  maximum  cant  angle  (Earth  reference) 

HCP 

Platform  maximum  cant  angle  yaw  (Platform  reference) 

This  report  assumes  the  reader  is  familiar  with  coordinate  systems,  rotation  matrices,  Euler 
angles,  etc.  For  convenience,  the  three  basic  rotation  matrices  are  provided  below.  The  subscripts 
indicate  the  axis  upon  which  the  rotation  is  being  applied.  Positive  rotation  is  defined  as  clockwise 
when  viewed  from  the  origin  outwards.  Additional  information  regarding  the  aforementioned  subjects 
can  be  found  in  reference  1 . 


cosij; 

— simj;  O' 
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Transformation  from  the  Earth  Referenced  Frame  to  the  Platform  Reference  Frame 

In  order  to  generate  a  rotation  matrix  relating  the  platform  reference  frame  to  the  Earth 
reference  frame,  one  must  perform  a  series  of  five  rotations  via  the  basic  rotation  matrices.  These 
rotations  describe  the  rotations  required  to  align  the  Earth  reference  frame  with  the  platform 
reference  frame. 


Rev  —  ^z(4fGe)^y'(0Ge)^x(cPGe)^y'(— 0Gp)^z(— ^Gp)  (4) 

The  transformation  begins  with  the  local  reference  frame  coincident  with  the  Earth  reference 
frame.  The  yaw,  pitch,  and  roll  angles  provided  by  the  gun  attitude  sensor  are  sequentially  applied  to 
rotations  about  the  Z,  Y,  and  X  axes  respectively.  Once  those  rotations  have  been  performed,  the 
local  reference  frame  is  aligned  with  the  gun  tube  (the  X  axis  is  coincident  with  the  centerline  of  the 
gun  tube).  At  this  point,  a  negative  rotation  about  the  Y  axis  is  applied  using  the  elevation  actuator 
angle.  This  rotation  aligns  the  local  reference  frame’s  XY  plane  with  the  platform  reference  frame’s 
XpYp  plane.  Finally,  a  negative  rotation  about  the  Z  axis  using  the  traverse  actuator  angle  fully  aligns 
the  local  reference  frame  with  the  platform  reference  frame. 

Substituting  the  basic  rotation  matrices  into  the  five  rotations  yields  equation  5.  Note  that  the 
sine  and  cosine  functions  are  represented  using  s  and  c  respectively  to  minimize  the  text  required. 


cikce 

O' 

C0Ge 

0 

S0Ge' 

1 

0 

0 

^®Gp 

0 

— s0Gp 

'-Clkcp 

S^Gp 

0 

Rev  — 

0 

0 

1 

0 

0 

C<Pce 

-S<PGe 

0 

1 

0 

-Sl|jGp 

-CI|JGP 

0 

.  0 

0 

1. 

S0Ge 

0 

C0Ge- 

0 

S<PGe 

CtPGe  - 

S^Gp 

0 

~  c0Gp 

■  0 

0 

1- 

Performing  the  matrix  multiplication  and  solving  using  the  values  provided  by  the  gun  attitude 
and  actuator  sensors  results  in  a  3x3  matrix  of  values  ranging  from  0  to  1.  The  columns  represent 
the  three  unit  vectors  that  form  the  Cartesian  coordinates  of  the  local  reference  frame  (aligned  with 
platform  reference  frame)  with  respect  to  the  Earth  reference  frame.  Equation  6  present  this  with  i,  j, 
and  k  representing  the  unit  vectors  of  the  X,  Y,  and  Z  axes.  The  Cartesian  coordinates  of  each  unit 
vector  are  represented  by  the  x,  y,  and  z  subscripts. 


Xx 

Yx 

Zx' 

j  = 

Yy 

k  = 

zy 

-Xz. 

. Yz . 

-Zz. 

Conversion  of  Rotation  Matrix  to  ZYX  Euler  Angles 

The  ZYX  Euler  angles  can  be  calculated  from  a  given  rotation  matrix  using  equations  7 
through  10.  The  inverse  kinematic  calculations  that  produced  these  equations  can  be  found  in 
reference  2. 

Given  a  rotation  matrix  represented  as: 


Mxx 

Myx 

Mzx 

MXy 

Myy 

MZy 

Mxz 

MYz 

Mzz 

(7) 
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0  =  atan2 


cp  =  atan2 


r  =  atan2 

\MXx' 

(8) 

~MXZ  \ 

MXxcos(\\j)  +  MXysm(\\))J 

(9) 

'MZx  sin(ijj)  -  MZycos(ij;)\ 

MYy  cos(ij;)  —  MYx sin(ijj)  J 

(10) 

Note  that  atan2  is  a  programming  function  that  uses  the  input  values  to  determine  the  correct 
quadrant  returned  from  an  arctangent  operation. 

For  the  unusual  case  where  the  platform  is  pointing  straight  up  or  straight  down,  the  angles 
are  as  follows: 


if((Wxx==0)&&(MXy==0)):  i|/  =  0,  0  =  |,  cp  =  atan2^)  (11) 

Calculation  of  Maximum  Platform  Cant  and  Heading 

Two  intersecting  planes  create  an  angle  between  them.  Of  equal  magnitude  is  the  angle 
formed  between  vectors  projecting  normally  from  their  respective  planes  and  originating  at  a  point 
along  the  line  of  intersection.  This  is  shown  in  figure  4  with  the  angle  0V  between  vectors  A7i  and  rti 
being  equal  to  the  angle  0P  formed  between  the  two  planes. 


Figure  3 

Intersecting  planes 

The  angle  between  two  vectors  is  equal  to  the  arc  cosine  of  the  dot  product  of  the  vectors 
divided  by  the  product  of  their  magnitudes. 
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0  =  acos 


(12) 


The  maximum  cant  of  a  platform  relative  to  Earth  is  represented  by  the  angle  between  the 
Earth  reference  frame’s  Z  axis  and  the  Z  axis  of  the  platform  reference  frame.  To  determine  this 
angle,  a  unit  vector  representing  the  platform  reference  frame’s  Z  axis  must  be  transformed  into  the 
Earth  reference  frame.  Since  the  cant  direction  sought  is  referenced  to  the  platform  reference  frame 
rather  than  the  Earth  reference  frame,  the  rotation  matrix  omits  the  traverse  rotation  about  the  Z  axis 


R  =  /?r(0pe)/?x(cppe)  (13) 


' COS0Pe 

0 

sinQ  Pe' 

1 

0 

0 

'  COS0Pe 

sinQpesirupPe 

sinBPecos(pPe~ 

0 

1 

0 

0 

coscppg 

—sim pPe 
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0 

coscppQ 

— sirupPe 

sm0Pe 

0 

COS0Pe_ 

0 

sirup  Pe 

COS(ppe 

sinQPe 

cosQPesirupPe 

COS0peCOS(ppe. 

The  transformed  Z  axis  unit  vector  is  the  product  of  the  rotation  matrix  and  the  Z  unit  vector: 


X 

O' 

sm0pecoscppe' 

y 

=  R 

0 

= 

— sincppe 

z 

.1. 

.cosGpgCoscppg. 

The  maximum  cant  angle  is  solved  using  equation  12.  In  this  case,  ^  is  Earth’s  Z  axis  unit  vector 
and  ri2  is  the  platform’s  transformed  Z  axis  unit  vector.  Since  the  vectors  represent  unit  vectors  of 
length  1 ,  the  equation  simplifies  to: 


( 

0 

Cpg  —  CLCOS  I 

0 

V 

.1. 

sm0Pecos(pPe 

—sirupPe 


(16) 


=  acos(cos0Pecos(pPe) 

The  traverse  angle  of  the  maximum  cant  relative  to  the  platform’s  heading  is  found  using  equation  8. 

(17) 


HCp  =  atan  2 


=  atan  2  (  ) 

v*y  \sm0pecos(ppe/ 


Example 

In  this  example,  the  gun  attitude  and  actuator  sensors  are  outputting  the  angles  shown  in 
table  2.  The  steps  required  to  convert  those  angles  to  platform  attitude  and  cant  angles  follow.  This 
example  represents  angles  using  North  Atlantic  T reaty  Organization  (NATO)  angular  mils  of  which 
there  are  6,400  in  a  circle. 
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Table  2 
Example  data 


Data 

Variable 

Value 

Gun  attitude  yaw  rotation  (Earth  reference) 

IflGe 

4,712  mils 

Gun  attitude  pitch  rotation  (Earth  reference) 

0Ge 

645  mils 

Gun  attitude  roll  rotation  (Earth  reference) 

(P  Ge 

-12  mils 

Gun  attitude  yaw  rotation  (Platform  reference) 

MJGp 

350  mils 

Gun  attitude  pitch  rotation  (Platform  reference) 

0Gp 

100  mils 

The  first  step  is  to  convert  the  units  from  mils  to  radians.  Conversion  in  both  directions  is 
shown  in  equations  18  and  19. 


Xr  =  2t r 
r  \6400/ 


(18) 


X 


m 


=  6400 


(19) 


The  resulting  values  are: 


i|iGe  =  4.626  rad 
0Ge  =  0.633  rad 
cpGe  =  —0.012  rad 
v|/Gp  =  0.344  rad 
0Gp  =  0.098  rad 


The  next  step  is  to  generate  the  rotation  transformation  matrix. 

R  =  /?z(4.626)flr(0.633  )/?z(-0.012)f?r(-0.344)/?z(-0.098)  (20) 


The  resulting  matrix  equals: 


R  = 


-0.4046 

-0.7803 

-0.4769 


0.9139 

-0.3635 

-0.1807 


-0.0323' 
-0.5090 
0.8602  . 


(21) 


Application  of  conversion  equations  8  through  10  yields: 


/-0.7803\  /-2.049\ 

ilipe  =  atan  2  ( - )  =  —2.049  rad  =  6400  ( - )  =  —2087  mils  =  4313  mils 

VFe  \— 0.4046/  V  2n  J 


(22) 
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0Pp  =  atan  2  ( - 

V-0. 


0.4769 


\  /0.477\ 

=  atan  2  -  =  .497  rad  =  506  mils 

)  V . 879  / 


4046  cos(ij;pe)  —  0.7803  sin(4>Pe) 


(23) 


cpPe  =  atan2 


—0.0323  sin(4>Pe)  +  0.5090cos(4»Pe)\ 
—0.3635  cos^pe)  —  0.9139sin(4»Pe)  J 


=  —0.207  rad  =  —211  mils 


(24) 


With  the  yaw,  pitch  and  roll  angles  of  the  platform  relative  to  Earth  determined,  the  cant  and 
cant  heading  can  be  determined  using  equations  16  and  17. 

CPe  =  acos(cos(.  497)cos(— 0.207))  =  .535  rad  =  545  mils  (25) 

(  —sin{— 0.207)  \  (26) 

HCd  =  atan  2  - — — — - - — -  -  =  0.415  rad  =  423  mils 

Lp  \sin(.  497)cos(— 0.207)  / 


RESULTS  AND  DISCUSSIONS 

The  equations  and  methods  presented  in  this  report  have  been  used  successfully  on  multiple 
systems.  Those  systems  used  an  Inertial  Navigation  Unit  to  provide  the  gun  to  Earth  Euler  angles 
and  absolute  encoders  to  provide  the  gun  to  platform  azimuth  and  elevation  angles.  The  weapon 
system  software  calculated  the  platform  attitude  at  a  periodic  interval,  providing  active  monitoring  of 
the  platform  attitude.  That  information  allowed  the  system  to  prevent  excessive  loads  on  the 
traversing  actuator  and  prohibit  firing  at  conditions  that  were  outside  safe  limits.  The  availability  of 
the  data  in  real  time  to  the  user  allows  for  positioning  of  the  platform  (vehicle)  in  an  acceptable 
orientation  for  operations. 

There  are  some  drawbacks  to  deriving  these  values  rather  than  measuring  them  directly  with 
dedicated  sensors.  Since  the  values  are  computed  rather  than  directly  measured,  their  accuracy  is  a 
function  of  the  five  input  sensors  and  the  round  off  errors  accumulated  from  the  multiple  calculations. 
The  amount  of  error  allowed  can  be  controlled  by  implementing  appropriately  rated  sensors  and 
performing  calculations  using  functions  of  an  adequate  precision  (i.e.,  use  double  precision  floating 
types).  Another  drawback  is  the  additional  load  on  the  computing  system.  There  are  several  floating 
point  operations  required  that  could  be  “expensive”  on  less  capable  processors. 

While  accuracy  and  processor  load  are  detractors  to  this  method,  they  can  be  accounted  for. 
There  is  significant  savings  in  cost,  weight,  and  power  consumption  realized  by  eliminating  the  need 
for  additional  sensor  units  and  cabling.  The  provided  equations  are  easily  implemented  in  software. 
MATLAB  code  has  been  provided  in  the  appendix  to  demonstrate  the  algorithms.  In  addition,  c  code 
is  available  from  the  author  upon  request. 


CONCLUSIONS 

Through  the  use  of  rotation  transformations  and  trigonometric  functions,  solutions  were  found 
for  the  aforementioned  problem.  These  calculations  allow  the  user  to  determine  platform  attitude  and 
maximum  cant  relative  to  Earth  without  the  need  for  additional  sensors. 
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MATLAB  Source  Code 


function  CantCalc 


clear 

format  bank 

GunE  =  struct (' Yaw ' ,  0,  'Pitch',  0,  'Roll',  0); 

PlatformE  =  struct (' Yaw ' ,  0,  'Pitch',  0,  'Roll',  0,  'Cant',  0,  'Heading',  0) ; 
Check  =  struct (' Yaw ' ,  0,  'Pitch',  0,  'Roll',  0); 

GunP  =  struct ('Az',  0,  'El',  0); 

running  =  1; 

M= [ 0 , 0 , 0 ; 

0,0,0; 

0,0,0]  ; 


while  (running  ==  1)  , 
clc 

%Print  current  data  to  screen 
fprintf ( 


fprintf ( ' 
fprintf ( ' 
fprintf ( ' 
fprintf ( ' 
fprintf ( ' 

fprintf ( ' 

fprintf ( ' 

fprintf ( ' 

fprintf ( ' 

fprintf ( ' 


Platform  Angles 

Calculation 

Gun  (E) 

1  Gun  (P) 

Platform  | 

Verify 

Yaw  %7.3f 

|  Az  %7.3f  |  Yaw 

%7.3f  |  Yaw 

%7.3f  | \n  ' ,  .. 

GunE. Yaw,  GunP.Az,  Plat formE . Yaw,  Check. Yaw); 


Pitch  %7.3f 


El  %7.3f 


Pitch 


%7.3f  |  Pitch  %7.3f  |\n'. 


GunE. Pitch,  GunP. El,  Plat formE . Pitch,  Check . Pitch) ; 


Roll  %7.3f 
GunE. Roll, 


|  Roll  %7.3f  |  Roll 
Plat formE . Roll,  Check. Roll 
%7.3f  | 


\n')  ; 
l\n') ; 
l\n') ; 
I \n ' ) ; 
I \n ' ) ; 


|  Cant 
PlatformE . Cant ) ; 
|  Heading  %7.3f 


7 . 3f  | \n ' , 

) ; 

I 


PlatformE . Heading) ; 

_l _ I. 


\n ' ,  ... 

\n ' ,  ... 

_l  \n\n ' ) 


%Print  menu  to  screen  and  get  command 
fprintf ('***  Command  Menu  ***\n'); 

fprintf (’1)  Enter  New  Gun-Earth  Attitude  (rads) \n' ) ; 
fprintf (’2)  Enter  New  Gun-Platform  Angles  (rads)Xn'); 
fprintf ('3)  Quit  Program\n\n ' ) ; 


mode  =  input ('Enter  Command:  '); 


switch  (mode) 
case  1 

%Get  Gun  Attitde  from  user  (relative  to  earth) 

GunE . Yaw=input (' Enter  Yaw  (rads):  '); 

GunE . Pitch=input (' Enter  Pitch  (rads):  '); 

GunE . Roll=input (' Enter  Roll  (rads):  '); 
case  2 

%Get  Gun  Orientation  from  user  (relative  to  platform) 
GunP .Az=input (' Enter  Azimuth  (rads):  '); 

GunP . El=input (' Enter  Elevation  (rads):  '); 
case  3 

running  =  0; 
break; 
otherwise 

disp (' Invalid  Command'); 
break; 

end 


%Create  Transformation  Matrix 
M=aboutz (GunE. Yaw)  ; 

M=M* about y (GunE . Pitch) ; 
M=M*aboutx (GunE . Roll)  ; 

M=M* about y (-GunP . El)  ; 

M=M* about z (-GunP.Az)  ; 
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%Generate  ypr  angles 

[PlatformE . Yaw, PlatformE .Pitch, PlatformE . Roll] =RtoYPR (M) ; 

%Generate  Cant  and  Heading 

PlatformE . Cant  =  acos (cos (Plat formE . Pitch)  *  cos (Plat formE . Roll) ) ; 
PlatformE . Heading  =  atan2 (-sin (PlatformE . Roll) ,  ... 

sin (PlatformE . Pitch  *  cos (PlatformE . Roll) )) ; 

%Create  new  transformation  Matrix  using  platform  angles 
M=aboutz (Plat formE . Yaw)  ; 

M=M*abouty (Plat formE . Pitch) ; 

M=M*aboutx (PlatformE . Roll)  ; 

M=M* about z (GunP.Az) ; 

M=M* about y (GunP . El)  ; 

%Generate  ypr  angles  of  new  rotation  transformation  using  platform  values 
[Check . Yaw, Check .Pitch, Check . Roll] =RtoYPR (M) ; 

end 

%Helper  functions 
function  [M] =aboutx (a) 

M=  [1,0,0; 

0,  cos (a) , -sin  (a) ; 

0,  sin (a) , cos  (a) ]  ; 

function  [M] =abouty (a) 

M=  [cos  (a)  ,  0,  sin  (a)  ; 

0,1,0; 

-sin (a) , 0, cos  (a) ] ; 

function  [M] =about z (a) 

M= [cos (a) , -sin (a) , 0; 
sin (a) , cos  (a)  ,  0; 

0,0,1]  ; 

function  [y, p, r ] =RtoYPR (M) 

if  M(2,l)  ==  0  &&  M(l,l)  ==  0  %  Pointing  straight  up  or  down, 

y  =  0; 
p  =  pi; 

r  =  atan2  (M(l,  2)  ,M(2, 2)  )  ; 

else 

y  =  atan2  (M (2, 1 )  , M (1, 1 )  )  ; 
cyaw  =  cos (y) ; 
syaw  =  sin  (y) ; 

p  =  atan2 (-M ( 3, 1 ) , M ( 1 , 1 ) *cyaw  +  M (2 , 1 ) *syaw) ; 

r  =  atan2 (M ( 1 , 3) *syaw  -  M (2 , 3) *cyaw, M (2 , 2 ) *cyaw  -  M ( 1 , 2 ) *syaw) ; 
if  (y  <  0) 

y  =  y  +  (2  *  pi)  ; 

end 

end 
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